/*
 *  Created on: Nov 15, 2012
 *      Author: Andrey Belomutskiy
 */
#include <stdarg.h>
#include <string.h>
#include <stdbool.h>
#include "rficonsole.h"


#ifndef CHIBIOS
 #include "rficonsole_stm32.h"
 typedef int32_t         bool_t;         /**< Fast boolean type.             */
#endif


int consoleActionCount = 0;
TokenCallback consoleActions[CONSOLE_MAX_ACTIONS];

void addConsoleAction1(char *token, void (*callback)(int)) {
	if (consoleActionCount == CONSOLE_MAX_ACTIONS) {
		print("Too many console actions\r\n");
		return;
	}
	TokenCallback *current = &consoleActions[consoleActionCount++];
	current->token = token;
	current->parameterType = ONE_PARAMETER;
	current->callback = callback;
}

void addConsoleAction(char *token, void (*callback)(void)) {
	if (consoleActionCount == CONSOLE_MAX_ACTIONS) {
		print("Too many console actions\r\n");
		return;
	}
	TokenCallback *current = &consoleActions[consoleActionCount++];
	current->token = token;
	current->parameterType = NO_PARAMETER;
	current->callback = callback;
}

int atoi(char *string) {
	int len = strlen(string);
	if (len == 0)
		return -ERROR_CODE;
	int result = 0;

	for (int i = 0; i < len; i++) {
		char ch = string[i];
		if (ch < '0' || ch > '9') {
			return ERROR_CODE;
		}
		int c = ch - '0';
		result = result * 10 + c;
	}

	return result;
}

#ifdef CHIBIOS
 #define PUT_CHAR(x) chIOPut(CONSOLE_CHANNEL, (uint8_t)(x))
#else
 #define PUT_CHAR(x) stmPutCh((x))
#endif


/**
 * @brief   System formatted output function.
 * @details This function implements a minimal @p printf() like functionality
 *          with output on a @p BaseChannel.
 *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
 *          The following parameter types (p) are supported:
 *          - <b>x</b> hexadecimal integer.
 *          - <b>X</b> hexadecimal long.
 *          - <b>o</b> octal integer.
 *          - <b>O</b> octal long.
 *          - <b>d</b> decimal signed integer.
 *          - <b>D</b> decimal signed long.
 *          - <b>u</b> decimal unsigned integer.
 *          - <b>U</b> decimal unsigned long.
 *          - <b>c</b> character.
 *          - <b>s</b> string.
 *          .
 * @note    Floating point types are not implemented, this function is meant
 *          as a system utility and not a full implementation.
 *
 * @param[in] chp       pointer to a @p BaseChannel implementing object
 * @param[in] fmt       formatting string
 */
void print(const char *fmt, ...) {
	va_list ap;
	char tmpbuf[_MAX_FILLER + 1];
	char *p, *s, c, filler;
	int i, precision, width;
	int is_long, left_align;
	long l;

	va_start(ap, fmt);
	while (TRUE) {
		c = *fmt++;
		if (c == 0) {
			va_end(ap);
			return;
		}
		if (c != '%') {
			PUT_CHAR(c);
			continue;
		}
		p = tmpbuf;
		s = tmpbuf;
		left_align = FALSE;
		if (*fmt == '-') {
			fmt++;
			left_align = TRUE;
		}
		filler = ' ';
		if (*fmt == '.') {
			fmt++;
			filler = '0';
		}
		width = 0;
		while (TRUE) {
			c = *fmt++;
			if (c >= '0' && c <= '9')
				c -= '0';
			else if (c == '*')
				c = va_arg(ap, int);
			else
				break;
			width = width * 10 + c;
		}
		precision = 0;
		if (c == '.') {
			while (TRUE) {
				c = *fmt++;
				if (c >= '0' && c <= '9')
					c -= '0';
				else if (c == '*')
					c = va_arg(ap, int);
				else
					break;
				precision *= 10;
				precision += c;
			}
		}
		/* Long modifier.*/
		if (c == 'l' || c == 'L') {
			is_long = TRUE;
			if (*fmt)
				c = *fmt++;
		} else
			is_long = (c >= 'A') && (c <= 'Z');

		/* Command decoding.*/
		switch (c) {
		case 'c':
			filler = ' ';
			*p++ = va_arg(ap, int);
			break;
		case 's':
			filler = ' ';
			if ((s = va_arg(ap, char *) ) == 0)
				s = "(null)";
			if (precision == 0)
				precision = 32767;
			for (p = s; *p && (--precision >= 0); p++)
				;
			break;
		case 'D':
		case 'd':
			if (is_long)
				l = va_arg(ap, long);
			else
				l = va_arg(ap, int);
			if (l < 0) {
				*p++ = '-';
				l = -l;
			}
			p = ltoa(p, l, 10);
			break;
		case 'X':
		case 'x':
			c = 16;
			goto unsigned_common;
		case 'U':
		case 'u':
			c = 10;
			goto unsigned_common;
		case 'O':
		case 'o':
			c = 8;
			unsigned_common: if (is_long)
				l = va_arg(ap, long);
			else
				l = va_arg(ap, int);
			p = ltoa(p, l, c);
			break;
		default:
			*p++ = c;
			break;
		}
		i = (int) (p - s);
		if ((width -= i) < 0)
			width = 0;
		if (left_align == FALSE)
			width = -width;
		if (width < 0) {
			if (*s == '-' && filler == '0') {
				PUT_CHAR(*s++);
				i--;
			}
			do
				PUT_CHAR(filler);
			while (++width != 0);
		}
		while (--i >= 0)
			PUT_CHAR(*s++);

		while (width) {
			PUT_CHAR(filler);
			width--;
		}
	}
}

#ifdef CHIBIOS
 #define GET_CHAR() (chIOGet(getSdChannel()))
#else
#define GET_CHAR() (0)
#endif

/**
 * @brief   Reads a whole line from the input channel.
 *
 * @param[in] line      pointer to the line buffer
 * @param[in] size      buffer maximum length
 * @return              The operation status.
 * @retval TRUE         the channel was reset or CTRL-D pressed.
 * @retval FALSE        operation successful.
 */
bool_t getConsoleLine(char *line, unsigned size) {
	char *p = line;

	while (TRUE) {
		short c = (short) GET_CHAR();
		if (c < 0)
			return TRUE;
		if (c == 4) {
			return TRUE;
		}
		if (c == 8) {
			if (p != line) {
				// backspace
				PUT_CHAR((uint8_t)c);
				PUT_CHAR(0x20);
				PUT_CHAR((uint8_t)c);
				p--;
			}
			continue;
		}
		if (c == '\r') {
			print("\r\n");
			*p = 0;
			return FALSE;
		}
		if (c < 0x20)
			continue;
		if (p < line + size - 1) {
			PUT_CHAR((uint8_t)c);
			*p++ = (char) c;
		}
	}
}


void consoleSayHello() {
#ifdef CHIBIOS
	print("+** ChibiOS/RT test suite\r\n");
	print("*** Kernel:       %s\r\n", CH_KERNEL_VERSION);
	print("*** Compiled:     " __DATE__ " - " __TIME__ " \r\n");
	print("%s\r\n", VERSION_STRING);
    print("CH_FREQUENCY=%d\r\n", CH_FREQUENCY);
#else
	print("Not as good as ChibiOS\r\n");
#endif
}

void help() {
	print("%d actions available:\r\n", consoleActionCount);
	int i;
	for (i = 0; i < consoleActionCount; i++) {
		TokenCallback *current = &consoleActions[i];
		print("  %s: %d parameters\r\n", current->token,
				current->parameterType);
	}
}

void echo(int value) {
	print("got value: %d\r\n", value);
}

// todo: this is ugly as hell!
static char consoleInput[] =
		"                                                                              ";
#ifdef CHIBIOS
static WORKING_AREA(consoleThread, 512);
static msg_t sdThreadEntryPoint(void *arg) {
	(void) arg;
	chRegSetThreadName("console thread");

	while (TRUE) {
		bool_t end = getConsoleLine(consoleInput,
				sizeof(consoleInput));
		if (end)
			break;

		handleConsoleLine(consoleInput);
	}
	return FALSE;
}
#endif

void initConsole() {
	/*
	 * Activates the serial using the driver default configuration (that's 38400)
	 * it is important to set 'NONE' as flow control! in terminal application on the PC
	 */
#ifdef CHIBIOS
	sdStart(CONSOLE_CHANNEL, NULL );

	palSetPadMode(CONSOLE_RX_PORT, CONSOLE_RX_PIN, PAL_MODE_ALTERNATE(CONSOLE_TX_AF));
	palSetPadMode(CONSOLE_TX_PORT, CONSOLE_TX_PIN, PAL_MODE_ALTERNATE(CONSOLE_TX_AF));

	chThdCreateStatic(consoleThread, sizeof(consoleThread), NORMALPRIO,
			sdThreadEntryPoint, NULL );
	print("STM32_PCLK1=%d\r\n", STM32_PCLK1);
#else
	enableSTM32console();
#endif

	addConsoleAction("help", &help);
	addConsoleAction("hello", &consoleSayHello);
	addConsoleAction1("echo", &echo);

	consoleSayHello();
}

#ifdef CHIBIOS
BaseChannel * getSdChannel() {
	return CONSOLE_CHANNEL;
}
#endif

void handleConsoleLine(char *line) {
	int firstTokenLength = tokenLength(line);
	int lineLength = strlen(line);

//	print("processing [%s] with %d actions\r\n", line, consoleActionCount);

	if (firstTokenLength == lineLength) {
		// no-param actions are processed here
		int i;
		for (i = 0; i < consoleActionCount; i++) {
			TokenCallback *current = &consoleActions[i];
			if (strEqual(line, current->token)) {
				// invoke callback function by reference
				(*current->callback)();
				return;
			}
		}
	} else {
		char *ptr = line + firstTokenLength;
		ptr[0] = 0; // change space into line end
		ptr++; // start from next symbol

///		print("with parameter [%s][%s]\r\n", line, ptr);
		int i;
		for (i = 0; i < consoleActionCount; i++) {
			TokenCallback *current = &consoleActions[i];
			if (strEqual(line, current->token)) {

				int value = atoi(ptr);
				if (value == ERROR_CODE) {
					print("invalid integer [%s]\r\n", ptr);
					return;
				}

				void (*callback1)(int) = current->callback;

				// invoke callback function by reference
				(*callback1)(value);
				return;
			}
		}
	}
	help();
}
